
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       srv_channel.c
  * @author     baiyang
  * @date       2021-12-29
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "srv_channel.h"

#include <rc_channel/rc_channel.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static uint16_t pwm_from_range(srv_channel* channel, float scaled_value);
static uint16_t pwm_from_angle(srv_channel* channel, float scaled_value);
/*----------------------------------variable----------------------------------*/
// mask of channels where we have a output_pwm value. Cleared when a
// scaled value is written. 
servo_mask_t srv_have_pwm_mask = ~(uint16_t)0;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void srv_channel_ctor(srv_channel* channel, uint8_t ch_num)
{
    channel->ch_num = ch_num;

    channel->servo_min = 1100;
    channel->servo_max = 1900;
    channel->servo_trim = 1500;

    // reversal, following convention that 1 means reversed, 0 means normal
    channel->reversed = 0;
    channel->function = 0;
}

void srv_channel_calc_pwm(srv_channel* channel, float output_scaled)
{
    if (srv_have_pwm_mask & (1U<<channel->ch_num)) {
        // Note that this allows a set_output_pwm call to override E-Stop!!
        // tricky to fix because we would endup E-stoping to individual SEROVx_MIN not MOT_PWM_MIN on copter
        return;
    }

    // check for E - stop
    bool force = false;
    if (srv_channel_should_e_stop(channel->function) && srvs.emergency_stop) {
        output_scaled = 0.0;
        force = true;
    }

    if (!force && channel->override_active) {
        // don't overwrite a override
        return;
    }

    if (channel->type_angle) {
        channel->output_pwm = pwm_from_angle(channel, output_scaled);
    } else {
        channel->output_pwm = pwm_from_range(channel, output_scaled);
    }
}

// return true if function is for anything that should be stopped in a e-stop situation, ie is dangerous
bool srv_channel_should_e_stop(Aux_servo_function_t function)
{
    switch (function) {
    case k_heli_rsc:  
    case k_heli_tail_rsc:
    case k_motor1:
    case k_motor2:
    case k_motor3:
    case k_motor4:
    case k_motor5:
    case k_motor6:
    case k_motor7:
    case k_motor8:
    case k_starter:
    case k_throttle:
    case k_throttleLeft:
    case k_throttleRight:
    case k_boost_throttle:
    case k_motor9:
    case k_motor10:
    case k_motor11:
    case k_motor12:
    case k_engine_run_enable:
        return true;
    default:
        return false;
    }
    return false;
}

void srv_channel_set_output_pwm(srv_channel* channel, uint16_t pwm, bool force)
{
    if (!channel->override_active || force) {
        channel->output_pwm = pwm;
        srv_have_pwm_mask |= (1U<<channel->ch_num);
    }
}

// set normalised output from -1 to 1, assuming 0 at mid point of servo_min/servo_max
void srv_channel_set_output_norm(srv_channel* channel, float value)
{
    // convert normalised value to pwm
    if (channel->type_angle) {
        srv_channel_set_output_pwm(channel, pwm_from_angle(channel, value * channel->high_out), false);
    } else {
        srv_channel_set_output_pwm(channel, pwm_from_range(channel, value * channel->high_out), false);
    }
}

// set angular range of scaled output
void srv_channel_set_angle(srv_channel* channel, int16_t angle)
{
    channel->type_angle = true;
    channel->high_out = angle;
    channel->type_setup = true;
}

// set range of scaled output
void srv_channel_set_range(srv_channel* channel, uint16_t high)
{
    channel->type_angle = false;
    channel->high_out = high;
    channel->type_setup = true;
}

/*
  get normalised output from -1 to 1, assuming 0 at mid point of servo_min/servo_max
 */
float srv_channel_get_output_norm(srv_channel* channel)
{
    uint16_t mid = (channel->servo_max + channel->servo_min) / 2;
    float ret;
    if (mid <= channel->servo_min) {
        return 0;
    }
    if (channel->output_pwm < mid) {
        ret = (float)(channel->output_pwm - mid) / (float)(mid - channel->servo_min);
    } else if (channel->output_pwm > mid) {
        ret = (float)(channel->output_pwm - mid) / (float)(channel->servo_max  - mid);
    } else {
        ret = 0;
    }
    if (channel->reversed != 0) {
        ret = -ret;
    }
    return ret;
}

uint16_t srv_channel_get_limit_pwm(srv_channel* channel, enum SRVLimit limit)
{
    switch (limit) {
    case SRV_TRIM:
        return channel->servo_trim;
    case SRV_MIN:
        return channel->reversed?channel->servo_max:channel->servo_min;
    case SRV_MAX:
        return channel->reversed?channel->servo_min:channel->servo_max;
    case SRV_ZERO_PWM:
        return 0;
    }
    return 0;
}

// return true if function is for a control surface
bool srv_channel_is_control_surface(Aux_servo_function_t function)
{
    switch (function)
    {
    case k_flap:
    case k_flap_auto:
    case k_aileron:
    case k_dspoilerLeft1:
    case k_dspoilerLeft2:
    case k_dspoilerRight1:
    case k_dspoilerRight2:
    case k_elevator:
    case k_rudder:
    case k_flaperon_left:
    case k_flaperon_right:
    case k_elevon_left:
    case k_elevon_right:
    case k_vtail_left:
    case k_vtail_right:
    case k_airbrake:
        return true;

    default:
        return false;
    }

    return false;
}

// return the motor number of a channel, or -1 if not a motor
// return 0 for first motor
int8_t srv_channel_get_motor_num(srv_channel* channel)
{
    const Aux_servo_function_t k_function = channel->function;
    switch (k_function) {
    case k_motor1:
    case k_motor2:
    case k_motor3:
    case k_motor4:
    case k_motor5:
    case k_motor6:
    case k_motor7:
    case k_motor8:
        return (int8_t)((uint16_t)(k_function) - k_motor1);
    case k_motor9:
    case k_motor10:
    case k_motor11:
    case k_motor12:
        return 8 + (int8_t)((uint16_t)(k_function) - k_motor9);
    default:
        break;
    }
    return -1;
}

/// map a function to a servo channel and output it
void srv_channel_output_ch(srv_channel* channel)
{
#ifndef HAL_BUILD_AP_PERIPH
    int8_t passthrough_from = -1;

    // take care of special function cases
    switch(channel->function)
    {
    case k_manual:              // manual
        passthrough_from = channel->ch_num;
        break;
    case k_rcin1: // rc pass-thru
    case k_rcin2:
    case k_rcin3:
    case k_rcin4:
    case k_rcin5: // rc pass-thru
    case k_rcin6:
    case k_rcin7:
    case k_rcin8:
    case k_rcin9: // rc pass-thru
    case k_rcin10:
    case k_rcin11:
    case k_rcin12:
    case k_rcin13: // rc pass-thru
    case k_rcin14:
    case k_rcin15:
    case k_rcin16:
        passthrough_from = (int8_t)((int16_t)channel->function - k_rcin1);
        break;
    default:
        break;
    }

    if (passthrough_from != -1) {
        // we are doing passthrough from input to output for this channel
        rc_channel_t c = rcs_chan(passthrough_from);
        if (c) {
            if (srvs.disabled_passthrough) {
                channel->output_pwm = c->radio_trim;
            } else {
                const int16_t radio_in = c->radio_in;
                if (!channel->ign_small_rcin_changes) {
                    channel->output_pwm = radio_in;
                    channel->previous_radio_in = radio_in;
                } else {
                    // check if rc input value has changed by more than the deadzone
                    if (abs(radio_in - channel->previous_radio_in) > c->dead_zone) {
                        channel->output_pwm = radio_in;
                        channel->ign_small_rcin_changes = false;
                    }
                }
            }
        }
    }
#endif // HAL_BUILD_AP_PERIPH

    if (!(srvs.disabled_mask & (1U<<channel->ch_num))) {
        srv_hal_write(channel->ch_num, channel->output_pwm);
    }
}

/*
   setup a channels aux servo function
*/
void srv_channel_aux_servo_function_setup(srv_channel* channel)
{
    if (channel->type_setup) {
        return;
    }
    switch (channel->function) {
    case k_flap:
    case k_flap_auto:
    case k_egg_drop:
        srv_channel_set_range(channel, 100);
        break;
    case k_heli_rsc:
    case k_heli_tail_rsc:
    case k_motor_tilt:
    case k_boost_throttle:
    case k_thrust_out:
        srv_channel_set_range(channel, 1000);
        break;
    case k_aileron_with_input:
    case k_elevator_with_input:
    case k_aileron:
    case k_elevator:
    case k_dspoilerLeft1:
    case k_dspoilerLeft2:
    case k_dspoilerRight1:
    case k_dspoilerRight2:
    case k_rudder:
    case k_steering:
    case k_flaperon_left:
    case k_flaperon_right:
    case k_tiltMotorLeft:
    case k_tiltMotorRight:
    case k_tiltMotorRear:
    case k_tiltMotorRearLeft:
    case k_tiltMotorRearRight:
    case k_elevon_left:
    case k_elevon_right:
    case k_vtail_left:
    case k_vtail_right:
    case k_scripting1:
    case k_scripting2:
    case k_scripting3:
    case k_scripting4:
    case k_scripting5:
    case k_scripting6:
    case k_scripting7:
    case k_scripting8:
    case k_scripting9:
    case k_scripting10:
    case k_scripting11:
    case k_scripting12:
    case k_scripting13:
    case k_scripting14:
    case k_scripting15:
    case k_scripting16:
    case k_roll_out:
    case k_pitch_out:
    case k_yaw_out:
        srv_channel_set_angle(channel, 4500);
        break;
    case k_throttle:
    case k_throttleLeft:
    case k_throttleRight:
    case k_airbrake:
        // fixed wing throttle
        srv_channel_set_range(channel, 100);
        break;
    default:
        break;
    }
}

servo_mask_t srv_channel_get_have_pwm_mask()
{
    return srv_have_pwm_mask;
}

void srv_channel_have_pwm_mask(uint8_t chan)
{
    srv_have_pwm_mask &= ~(1U<<chan);
}

// convert a 0..range_max to a pwm
static uint16_t pwm_from_range(srv_channel* channel, float scaled_value)
{
    if (channel->servo_max <= channel->servo_min || channel->high_out == 0) {
        return channel->servo_min;
    }
    scaled_value = math_constrain_float(scaled_value, 0, channel->high_out);
    if (channel->reversed) {
        scaled_value = channel->high_out - scaled_value;
    }
    return channel->servo_min + (uint16_t)( (scaled_value * (float)(channel->servo_max - channel->servo_min)) / (float)channel->high_out );
}

// convert a -angle_max..angle_max to a pwm
static uint16_t pwm_from_angle(srv_channel* channel, float scaled_value)
{
    if (channel->reversed) {
        scaled_value = -scaled_value;
    }
    scaled_value = math_constrain_float(scaled_value, -channel->high_out, channel->high_out);
    if (scaled_value > 0) {
        return channel->servo_trim + (uint16_t)( (scaled_value * (float)(channel->servo_max - channel->servo_trim)) / (float)channel->high_out);
    } else {
        return channel->servo_trim - (uint16_t)( (-scaled_value * (float)(channel->servo_trim - channel->servo_min)) / (float)channel->high_out);
    }
}

/*------------------------------------test------------------------------------*/


